#include <ros/ros.h>
#include <std_msgs/String.h>
#include <relative_move/SetRelativeMove.h>
#include <ar_pose/Track.h>
#include <iostream>

class AutoCharge {
public:
    AutoCharge();
    void start();

private:
    void signalCallback(const std_msgs::String::ConstPtr& msg);
    void performSecondMove();

    ros::NodeHandle n;
    ros::ServiceClient relative_move_client;
    ros::ServiceClient ar_track_client;
    ros::Subscriber signal_sub;
    ros::Publisher charging_status_pub; // 新增的Publisher
};

AutoCharge::AutoCharge() {
    relative_move_client = n.serviceClient<relative_move::SetRelativeMove>("relative_move");
    ar_track_client = n.serviceClient<ar_pose::Track>("track");
    signal_sub = n.subscribe("xinhao", 10, &AutoCharge::signalCallback, this);
    charging_status_pub = n.advertise<std_msgs::String>("charging_status", 10); // 初始化Publisher
}

void AutoCharge::start() {
    ros::service::waitForService("relative_move");
    ros::service::waitForService("track");
    ros::spin(); // 保持节点运行，等待订阅消息
}

void AutoCharge::signalCallback(const std_msgs::String::ConstPtr& msg) {
    if (msg->data == "start_second_move") {
        ROS_INFO("Received start_second_move signal.");
        performSecondMove();
    }
}

void AutoCharge::performSecondMove() {
    ar_pose::Track Track_data;
    Track_data.request.ar_id = 0; // 跟踪0号ar码
    Track_data.request.goal_dist = 0.4;
    ar_track_client.call(Track_data); // 调用服务

    relative_move::SetRelativeMove RelativeMove_data;
    RelativeMove_data.request.goal.x = -0.2;
    RelativeMove_data.request.global_frame = "odom";
    relative_move_client.call(RelativeMove_data); // 调用服务

    std::cout << "定位完成" << std::endl;

    // 发布充电完成的消息
    std_msgs::String status_msg;
    status_msg.data = "charging_complete";
    charging_status_pub.publish(status_msg);
    ROS_INFO("Published charging_complete signal to charging_status topic.");

    std::cout << "现在退出程序" << std::endl;
    ros::Duration(30.0).sleep();
    status_msg.data = "recover";
    charging_status_pub.publish(status_msg);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "auto_charge");
    AutoCharge auto_charge;
    auto_charge.start();
    return 0;
}

